#ifndef _TRAJECTORY_GENERATOR_OSQP_H_
#define _TRAJECTORY_GENERATOR_OSQP_H_

#include <Eigen/Eigen>
#include <vector>

#include "TimeAllocation.h"

class TrajectoryGeneratorOsqp
{
private:
    // double _qp_cost;
    Eigen::MatrixXd _Q;
    
    Eigen::VectorXd _Px, _Py, _Pz;

    Eigen::MatrixXd getQ(const int p_num1d,
                         const int order, 
                         const TimeAllocation &Time, 
                         const int seg_index);

    std::pair<MatrixXd,VectorXd> getAbeq(const int d_order,           // the order of derivative
                                        const Eigen::VectorXd &WayPoints1D,
                                        const Eigen::VectorXd &StartState1D,
                                        const Eigen::VectorXd &EndState1D,
                                        const TimeAllocation &Time);

    Eigen::VectorXd OSQPCalCoeff1D(const Eigen::MatrixXd &Q,
                                   const Eigen::VectorXd &WayPoints1D,
                                   const Eigen::VectorXd &StartState1D,
                                   const Eigen::VectorXd &EndState1D,
                                   const TimeAllocation &Time,
                                   const int d_order);

    int Factorial(int x);
public:
    TrajectoryGeneratorOsqp();

    ~TrajectoryGeneratorOsqp();

    Eigen::MatrixXd PolyQPGeneration(
        const int order,
        const Eigen::MatrixXd &Path,
        const Eigen::MatrixXd &Vel,
        const Eigen::MatrixXd &Acc,
        const TimeAllocation &Time);

    bool testQP();
};

#endif
